#define COUNT_PER_CYCLE 20   // how many count when encoder runs one cycle
#define N_SAMPLES 5       // how many samples the array carrys


static long lastTime = 0;
static unsigned long intervals[N_SAMPLES];   // time intervals between two falling edge
static byte curr_pos = 0;            // where the current sample is stored


/* ===================================
    Definitions
   =================================== */
// Add a time interval sample, intended to be called
// every time a pulse is detected.
void Sample() {
  curr_pos++;
  if (curr_pos >= N_SAMPLES) {
    curr_pos = 0;
  }

  unsigned long _thisTime = micros();
  intervals[curr_pos] = _thisTime - lastTime;
  lastTime = _thisTime;  
}


// return the sum of time intervals
static long Sum() {
  long _total = 0;
  for (int i=0; i<N_SAMPLES; i++) {
    _total += intervals[i];
  }

  return _total;
}


float ReadSpeed() {
  // speed = revolution / time = (N_SAMPLES / COUNT_PER_CYCLE) / (millis / 1000000)
  return (1000000.0 * (float)N_SAMPLES / (float)COUNT_PER_CYCLE) / (float)Sum();
}
